﻿using CSPCI1000;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Threading;
using System.Threading.Tasks;

namespace Scaffold
{

	public class PCIBoard //:IDisposable
	{
		public class Speed
		{
			public int VMax, VMin;
			public float Tacc;
			public Speed(int vmin, int vmax, float tacc)
			{
				VMax = vmax; VMin = vmin; Tacc = tacc;
			}
		}

		private static int __TimeInterval = 100;
		private int __cardNO;       //卡号

		private Point loc;
		private double XMcPerpx
		{
			get
			{
				return Config.config.XMCPerPX;
			}
		}
		private double YMcPerpx
		{
			get
			{
				return Config.config.YMCPerPX;
			}
		}
		private double McPerdg
		{
			get
			{
				return Config.config.MCPerDG;
			}
		}
		private double McPermm
		{
			get
			{
				return Config.config.MCPerMM;
			}
		}
		public float AngOffset
		{
			get
			{
				return Config.config.AngOff;
			}
		}
		public enum AXIS
		{
			X = 0, Y, Z, R
		};
		public static uint GlobalInit()
		{
			uint x = PCIInvoke.w1000_initial();
			Console.WriteLine($"初始化...找到{x}个控制卡");
			return x;
		}

		public PCIBoard(int cardNo = 0)
		{
			loc = new Point();
			//Todo: 注意这里范围可能得视情况修改。MoveHome、Stop、wait、setplsmode也是
			for (AXIS i = AXIS.X; i <= AXIS.Y; i++)
			{
				//脉冲模式1，脉冲下降沿有效
				PCIInvoke.w1000_set_pls_outmode((ushort)(4 * cardNo + i), 1);
				//EL减速模式：0表示立即停止，1减速停止
				PCIInvoke.w1000_set_el((ushort)(4 * cardNo + i), 1);
				//减速信号，0表示无效，1表示使能
				PCIInvoke.w1000_set_sd((ushort)(4 * cardNo + i), 0);
				//ORG减速模式： 0 表示立即停止，1减速停止
				PCIInvoke.w1000_set_org((ushort)(4 * cardNo + i), 1);
			}
			__cardNO = cardNo;
		}
		public void SetPlsMode(ushort mode, int axis = -1)
		{
			if (axis < 0)
				for (AXIS i = AXIS.X; i <= AXIS.Y; i++)
				{
					PCIInvoke.w1000_set_pls_outmode((ushort)(4 * __cardNO + i), mode);
				}
			else
				PCIInvoke.w1000_set_pls_outmode((ushort)(4 * __cardNO + axis), mode);
		}
		public uint MoveHome(Speed speed, int slowSpeed, double tacc)
		{
			Console.WriteLine("回原点...");
			loc = new Point();
			var p = new List<ushort> { 5, 6 };
			var axes = new List<AXIS> { AXIS.X, AXIS.Y };

			new Task(() => {
				if (Read(p[0]) == 1)//如果输入无信号
				{
					Console.WriteLine($"高速返回, 轴{axes[0]}");
					PCIInvoke.w1000_sv_move((ushort)(4 * __cardNO + axes[0]), -speed.VMin, -speed.VMax, speed.Tacc);
					WaitForInputState(p[0], true);
					PCIInvoke.w1000_sd_stop((ushort)(4 * __cardNO + axes[0]));
					WaitForMotionDone(new List<AXIS> { axes[0] });
				}
				Console.WriteLine($"低速正方向移动, , 轴{axes[0]}");
				PCIInvoke.w1000_sv_move((ushort)(4 * __cardNO + axes[0]), 0, slowSpeed, tacc);
				WaitForInputState(p[0], false);
				PCIInvoke.w1000_emg_stop((ushort)(4 * __cardNO + axes[0]));

				if (Read(p[1]) == 1)//如果输入无信号
				{
					Console.WriteLine($"高速返回, 轴{axes[1]}");
					PCIInvoke.w1000_sv_move((ushort)(4 * __cardNO + axes[1]), -speed.VMin, -speed.VMax, speed.Tacc);
					WaitForInputState(p[1], true);
					PCIInvoke.w1000_sd_stop((ushort)(4 * __cardNO + axes[1]));
					WaitForMotionDone(new List<AXIS> { axes[1] });
				}
				Console.WriteLine($"低速正方向移动, , 轴{axes[1]}");
				PCIInvoke.w1000_sv_move((ushort)(4 * __cardNO + axes[1]), 0, slowSpeed, tacc);
				WaitForInputState(p[1], false);
				PCIInvoke.w1000_emg_stop((ushort)(4 * __cardNO + axes[1]));
			}).Start();
			return 0;
		}
		public uint MoveXY(Point t, Point offset, Speed speed, bool turnBeforeMove = true)
		{
			t.Offset(offset);
			double angle = Math.Atan2(t.Y - loc.Y, t.X - loc.X) + Math.PI;
			if (turnBeforeMove)
			{
				var x3 = TurnTo(angle / Math.PI * 180, speed);
				Console.WriteLine($"转向: {angle / Math.PI * 180}度");
			}
			Console.WriteLine($"从像素位置({loc.X},{loc.Y}) 移到 ({t.X},{t.Y})). X方向{(int)((t.X - loc.X) * XMcPerpx)}脉冲, Y方向{(int)((t.Y - loc.Y) * YMcPerpx)}脉冲。");
			SetPlsMode(1, (int)AXIS.X);
			SetPlsMode(1, (int)AXIS.Y);
			var x1 = PCIInvoke.w1000_s_move((ushort)(4 * __cardNO + AXIS.X), (int)((t.X - loc.X) * XMcPerpx), (int)(speed.VMin * Math.Cos(angle)), (int)(speed.VMax * Math.Cos(angle)), speed.Tacc);
			var x2 = PCIInvoke.w1000_s_move((ushort)(4 * __cardNO + AXIS.Y), (int)((t.Y - loc.Y) * YMcPerpx), (int)(speed.VMin * Math.Sin(angle)), (int)(speed.VMax * Math.Sin(angle)), speed.Tacc);
			loc = t;
			if (x1 != PCIInvoke.ERRCode.NoError)
				return (uint)x1;
			if (x2 != PCIInvoke.ERRCode.NoError)
				return (uint)x2;
			WaitForMotionDone(new List<AXIS> { AXIS.X, AXIS.Y});
			// Console.WriteLine("按任意键继续...");Console.ReadLine();
			return 0;
		}

		public uint KeepMove(AXIS axis, Speed speed)
		{
			return (uint)PCIInvoke.w1000_sv_move((ushort)(4 * __cardNO + axis), speed.VMin, speed.VMax, speed.Tacc);
		}


		/// <summary>
		/// 移动Z轴
		/// </summary>
		/// <param name="h">绝对位置。单位毫米</param>
		public uint MoveZ(double h, Speed speed)
		{
			//SetPlsMode(0,(int)AXIS.Z);
			SetPlsMode(1, (int)AXIS.Z);
			Console.WriteLine($"Z绝对位置(脉冲)：{(int)(h * McPermm)}");
			var x = PCIInvoke.w1000_sa_move((ushort)(4 * __cardNO + AXIS.Z), (int)(h * McPermm), speed.VMin, speed.VMax, speed.Tacc);
			if (x != PCIInvoke.ERRCode.NoError)
				return (uint)x;
			WaitForMotionDone(new List<AXIS> { AXIS.Z });
			return 0;
		}
		public uint TurnTo(double d, Speed speed)
		{
			d += AngOffset;
			SetPlsMode(1, (int)AXIS.R);
			Console.WriteLine($"  转向{d}度，脉冲数：{(int)(d * McPerdg)}");
			var x = PCIInvoke.w1000_sa_move((ushort)(4 * __cardNO + AXIS.R), (int)(d * McPerdg), speed.VMin, speed.VMax, speed.Tacc);
			if (x != PCIInvoke.ERRCode.NoError)
				return (uint)x;
			WaitForMotionDone(new List<AXIS> { AXIS.R });
			return 0;
		}
		/// <summary>
		/// 打开/关闭指定输出口
		/// </summary>
		/// <param name="port">输出口</param>
		/// <param name="data">true-打开;false-关闭</param>
		/// <returns></returns>
		public uint Output(ushort port, bool data)
		{
			//这个函数0表示高，1表示低。
			var result = PCIInvoke.w1000_write_do(port, Convert.ToUInt16(!data));
			Console.WriteLine($"{port}号开关 {(data ? "打开" : "关闭")}，操作结果：{result}");
			return result;
		}

		public uint Read(ushort port, bool inputType = true)
		{
			//对于输入口状态：返回0表示有输入。1表示无输入。
			uint result = 0;
			if (inputType)
				result = PCIInvoke.w1000_read_di(port);
			else
				result = PCIInvoke.w1000_read_do(port);
			return result;
		}
		public void WaitForMotionDone(List<AXIS> axes)
		{
			List<bool> stoped = new List<bool>();
			foreach (var item in axes)
			{
				stoped.Add(false);
			}

			int T = axes.Count;
			for (; T > 0;)
			{
				Thread.Sleep(__TimeInterval);
				for (int i = 0; i < axes.Count; i++)
					if (!stoped[i])
					{
						var e = PCIInvoke.w1000_motion_done((ushort)(4 * __cardNO + axes[i]));
						//Console.WriteLine($" 等待 {(ushort)(4 * __cardNO + axes[i])} 号轴，结果： {(e != 0 ? "停止" : "运动")}");
						if (e != 0)
						{
							stoped[i] = true;
							T--;
						}
					}
			}
		}
		public int WaitForInputState(ushort port, bool targetState, int maxWaitTime = -1)
		{
			int tm = 0;
			Console.Write($"等待{port}号输入 {(targetState ? "有" : "无")}信号...");
			for (; maxWaitTime < 0 || tm + __TimeInterval <= maxWaitTime;)
			{
				Thread.Sleep(__TimeInterval);
				tm += __TimeInterval;
				if ((Read(port, true) == 0) == targetState)
				{
					Console.WriteLine("OK!");
					break;
				}
			}
			return tm;
		}

		public void Stop()
		{
			PCIInvoke.w1000_sd_stop((ushort)(4 * __cardNO + AXIS.X));
			PCIInvoke.w1000_sd_stop((ushort)(4 * __cardNO + AXIS.Y));
		}

	}
}
